#pragma once
#include<iostream>
#include<cstdio>
#include<vector>
#include<math.h>
#include<fstream>


#pragma once
#include "CFilter.h"
#include "CTorqueControl.h"
#include "CurrentInnerCon.h"
#include "CPositionControl.h"
#include "VelCtrl.h"
#include "PosCtrlVer_Vel.h"
#include "CMeasure.h"
#include "windows.h"
#include "process.h"

#include "CML.h"
#include "can_copley.h"

using namespace std;

#define AMPCT 1			//Number of Amplifer
#define LINE 1024*4     //Encoder line
#define TIMER_PERIOD 5
#define DEFAULT_KP 37
#define DEFAULT_KD 0.3
#define DEFAULT_PASS_FREQ 10
#define DEFAULT_ELASTIC_COIFFICEINT 1.48

#define STATE_STOP		0
#define STATE_HOMING	1	
#define STATE_CONTROL	2


const int32 canBPS=1000000;	//CAN network bit rate
//const Error *err;	// const Error *Open( CanInterface &can );
//int16 canNodeID=2;	//canopen node ID of first amp,second will increase by 1;




class CLowLevelControllerClass
{
public:
	CLowLevelControllerClass(void);
	~CLowLevelControllerClass(void);

	void Init();
	void GoToZeroPosition();
	void StartControl();
	void StopControl();

	int state;
	
	
	CanOpen canOpen;  // class CanOpen
	CopleyCAN can0,can1;	//This class extends the generic CanInterface class into a working interface for the Copley can device driver.
	void showerr(const Error *err,char *str);
	void ReadSensorData();
	
	const Error *err;	// const Error *Open( CanInterface &can );
	int16 canNodeID;	//canopen node ID of first amp,second will increase by 1;
	CanFrame frame;	//contains the information of can data package;
	CanFrame clear_frame;
	CSeaAmp amp[AMPCT];
	Linkage link;
	CFilter ampFilter[AMPCT];
	CTorqueControl ampTorqueControl[AMPCT];
	CMeasure ampMeasure[AMPCT];
	CPositionControl ampPositionControl[AMPCT];
	CCurrentInnerCon ampCurrentInnerCon[AMPCT];
	CVelCrtl  ampVelCrtl[AMPCT];
	CPosCtrlVer_Vel ampPosCtrlVer_Vel[AMPCT];

	

};

static void SensorStarter(void *llc)
{

	((CLowLevelControllerClass*)llc)->ReadSensorData();


}
